#include "SERVO/servo_app.h"
#include "math.h"
#include "stdio.h"
#include "stdlib.h"

SERVO_PWM_HandleTypeDef htim_pwm_horizontal;       // PWM 定时器句柄
SERVO_PWM_HandleTypeDef htim_pwm_vertical;       

Servo_Params Servo;

int16_t New_angle = 90;

int16_t horizontal_angle = 90;
int16_t vertical_angle = 90;

#define Angle_division_value 3

#define PULSE_PER_DEGREE ((SERVO_MAX_PULSE - SERVO_MIN_PULSE) / 180.0f)

/**
 ****************************************************************************************************
 * DONE 用户函数区
 ****************************************************************************************************
 */

/**
 * @name pwm初始化函数
 * @brief 初始化舵机pwm引脚
 * @param htim_pwm；pwm句柄
 * @param htim：定时器句柄
 * @param Channel：pwm所用的通道号
 */
void Servo_PWM_Init(SERVO_PWM_HandleTypeDef *htim_pwm, TIM_HandleTypeDef *htim, uint32_t Channel) 
{
    htim_pwm->htim = htim;
    htim_pwm->Channel = Channel;
    HAL_TIM_PWM_Start(htim, Channel);
    // __HAL_TIM_SET_COUNTER(htim, 0);
}

/**
 * @name 舵机初始化函数
 * @brief 对舵机的初始化
 */
void Servo_Init(void)
{
    Servo.max_pulse = SERVO_MAX_PULSE;
    Servo.min_pulse = SERVO_MIN_PULSE;
    Servo.period = SERVO_PERIOD;

    Servo_PWM_Init(&htim_pwm_horizontal,&htim2, TIM_CHANNEL_1);
    Servo_PWM_Init(&htim_pwm_vertical,&htim2, TIM_CHANNEL_2);
}

/**
 * @brief 设置舵机角度
 * @param angle 角度值 (0°-180°)
 * @param servo_id 舵机编号 (1 或 2)
 */
void Servo_SetAngle(float angle, Servo_Direction servo_id)
{
    uint32_t pulse_width;

    // 将角度转换为脉宽 (1ms-2ms 对应 0°-180°)
    if (angle < 0) angle = 0;
    if (angle > 180) angle = 180;
    pulse_width = Servo.min_pulse + (uint32_t)(angle * PULSE_PER_DEGREE);

    // 根据舵机编号设置对应的 TIM4 通道
    switch (servo_id) {
        case SERVO_horizontal:
            __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, pulse_width);
            break;
        case SERVO_vertical:
            __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2, pulse_width);
            break;
    }

    if(servo_id != SERVO_horizontal && servo_id != SERVO_vertical)
    {
        return; 
    }
}

/**
 * @name 舵机控制函数
 * @brief 对舵机进行精密控制
 * @param joy_val：摇杆值（0-255）
 * @param servo_id：舵机编号，SERVO_horizontal为水平转舵机，SERVO_vertical为竖直转舵机
 * @param Control_mode：控制模式，0为摇杆方向控制，1为摇杆值映射控制
 */
void Servo_Control_Advanced(uint16_t joy_val, Servo_Direction servo_id,uint16_t Control_mode) 
{
    if(Control_mode)
    {
        static float filtered_angle = 90;  // 保存滤波后的角度（初始值90°）
        const float alpha = 0.3;          // 滤波系数（0.1-0.3）
    
        //摇杆值归一化：将原始值[0,255]转换为[-1,1], 摇杆中值127对应0，两侧极值对应-1和1 
        float norm_val = (joy_val - 127) / 127.0f;
    
        //指数曲线变换：增强小幅度输入的灵敏度, 公式：y = x * |x|，使小输入更敏感，大输入非线性增长 
        float exp_val = norm_val * fabs(norm_val);
    
        //映射到目标角度：[-1,1] → [0°,180°], 中位0对应90°，-1对应0°，1对应180°
        float target_angle = 90 + (exp_val * 90);
    
        //低通滤波：抑制高频抖动, 公式：filtered = α * target + (1-α) * last_filtered 
        filtered_angle = alpha * target_angle + (1-alpha) * filtered_angle;
    
        //输出到舵机 
        Servo_SetAngle(filtered_angle,servo_id);
    }
    else
    {
        //摇杆值归一化：将原始值[0,255]转换为[-1,1], 摇杆中值127对应0，两侧极值对应-1和1 
        float norm_val = (joy_val - 127) > 0 ? 1 : -1;
        if(servo_id == SERVO_horizontal) 
        {
            norm_val *= -1;
            horizontal_angle += norm_val*Angle_division_value;
            if(horizontal_angle > 180) horizontal_angle = 180;
            if(horizontal_angle < 0) horizontal_angle = 0;  
            Servo_SetAngle(horizontal_angle,servo_id); 
        }
        else if(servo_id == SERVO_vertical) 
        {
            vertical_angle += norm_val*Angle_division_value;
            if(vertical_angle > 180) vertical_angle = 180;
            if(vertical_angle < 80) vertical_angle = 80;
            Servo_SetAngle(vertical_angle,servo_id); 
        }             

    }

}

void Servo_Control_vertical(uint16_t joy_val) 
{
    uint32_t pulse_width;
    //摇杆值归一化：将原始值[0,255]转换为[-1,1], 摇杆中值127对应0，两侧极值对应-1和1 
    float norm_val = (joy_val - 127) > 0 ? 1 : -1;

    New_angle += norm_val*Angle_division_value;
    if(New_angle > 180) New_angle = 180;
    if(New_angle < 80) New_angle = 80; 

    // 将角度转换为脉宽 (1ms-2ms 对应 0°-180°)
    pulse_width = Servo.min_pulse + (uint32_t)(New_angle * PULSE_PER_DEGREE);

    // 根据舵机编号设置对应的 TIM4 通道
    __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2, pulse_width);   
}

void Servo_Control_horizontal(uint16_t joy_val) 
{
    uint32_t pulse_width;
    //摇杆值归一化：将原始值[0,255]转换为[-1,1], 摇杆中值127对应0，两侧极值对应-1和1 
    float norm_val = (joy_val - 127) > 0 ? 1 : -1;

    New_angle += norm_val*Angle_division_value;
    if(New_angle > 180) New_angle = 180;
    if(New_angle < 0) New_angle = 0; 

    // 将角度转换为脉宽 (1ms-2ms 对应 0°-180°)
    pulse_width = Servo.min_pulse + (uint32_t)(New_angle * PULSE_PER_DEGREE);
    
    // 根据舵机编号设置对应的 TIM4 通道
     __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, pulse_width);   
}
